﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    public static class PhysicsUtil
    {
        const int BVH_CACHE_ARRAY_COUNT = 20;
        const float VIRTUAL_RAY = 10__0000;

        public struct RaycastHit
        {
            public PhysicsBase Object { get; set; }
            public Vector3 ContactPoint { get; set; }
        }

        public struct StepMoveOptions
        {
            public float StepLength;
            public float Eps;
            public int MaxIterateCount;
            public int IntersectDeepIterateCount;
            public int ObstacleLayerMask;

            public bool UseBvhQuery;
            public int ObstacleBvhID;

            public bool UseOnGroundDetection;


            public StepMoveOptions DetectOnGround()
            {
                UseOnGroundDetection = true;

                return this;
            }

            public StepMoveOptions SetBvh(int obstacleBvhID)
            {
                UseBvhQuery = true;
                ObstacleBvhID = obstacleBvhID;

                return this;
            }

            public static StepMoveOptions Create(float stepLength = 0.3f, int obstacleLayerMask = ~0, int intersectDeepIterateCount = 2)
            {
                return new StepMoveOptions()
                {
                    StepLength = stepLength,
                    Eps = 0.05f,
                    MaxIterateCount = 600,
                    IntersectDeepIterateCount = intersectDeepIterateCount,
                    ObstacleLayerMask = obstacleLayerMask,
                };
            }
        }

        public static readonly float INFINITY = 100000.0f;
        public static readonly Color PHYSICS_SHAPE_GIZMOS_COLOR = new Color(145f, 244f, 139f, 210f) / 255f;

        static PhysicsBase[] mBVHQueryCacheArray;

        static SphereShape CheckSphereTestShape;
        static BoxShape CheckBoxTestShape;
        static VirtualCapsuleShape CheckCapsuleTestShape;

        static BoxShape StepMoveCastBoxTestShape;


        static PhysicsUtil()
        {
            mBVHQueryCacheArray = new PhysicsBase[BVH_CACHE_ARRAY_COUNT];

            CheckSphereTestShape = new SphereShape() { matrix = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one) };
            CheckBoxTestShape = new BoxShape() { matrix = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one) };
            CheckCapsuleTestShape = new VirtualCapsuleShape() { matrix = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one) };

            StepMoveCastBoxTestShape = new BoxShape() { matrix = Matrix4x4.TRS(Vector3.zero, Quaternion.identity, Vector3.one) };
        }

        public static bool StepMoveTo(PhysicsObject physicsObject, Vector3 to, StepMoveOptions options)
        {
            if (physicsObject.PhysicsShape.IsContainPoint(to)) return false;

            var edgePoint = Vector3.zero;

            edgePoint = physicsObject.PhysicsShape.MassPointToEdge(physicsObject.transform.position, to);

            var moveValue = to - edgePoint;
            var firstIntersect = default(IPhysicsShape);

            return StepMove(physicsObject, moveValue, out firstIntersect, options);
        }

        public static bool StepMove(PhysicsObject physicsObject, Vector3 moveValue, StepMoveOptions options)
        {
            var firstIntersect = default(IPhysicsShape);

            return StepMove(physicsObject, moveValue, out firstIntersect, options);
        }

        public static bool StepMove(PhysicsObject physicsObject, Vector3 moveValue, out IPhysicsShape firstIntersect, StepMoveOptions options)
        {
            var result = true;
            firstIntersect = default(IPhysicsShape);

            var bvhContainer = options.UseBvhQuery ? BVHTreeContainer.GetContainerWithID(options.ObstacleBvhID) : default(BVHTreeContainer);

            var direction = moveValue.normalized;
            var stepLength = options.StepLength;
            var step = direction * stepLength;

            if (step.magnitude > moveValue.magnitude)
                step = moveValue;

            var to = physicsObject.transform.position + moveValue;
            var errorCounter = 0;

            var currentDeepIterateCount = 1;
            var canNext = true;
            while (canNext)
            {
                errorCounter++;
                if (errorCounter > options.MaxIterateCount)
                {
                    Debug.LogError("StepMove While Crashed!");
                    result = false;
                    break;
                }
                //如果超出最大迭代数量就报错

                var flag = false;
                var cachePosition = physicsObject.transform.position;
                physicsObject.transform.position += step;

                if (bvhContainer != null)
                {
                    var physicsShape = physicsObject.PhysicsShape;
                    var selfBounds = physicsShape.Bounds;
                    var count = bvhContainer.Tree.Intersect(selfBounds, mBVHQueryCacheArray);
                    for (int i = 0; i < count; i++)
                    {
                        var queryShape = mBVHQueryCacheArray[i].PhysicsShape;
                        if (queryShape.Bounds.Intersects(selfBounds)
                            && queryShape.IntersectsTest(physicsShape))
                        {
                            flag = true;
                            break;
                        }
                    }
                }
                else
                {
                    flag = PhysicsSystemManager.Instance.IsIntersectWithSceneColliders(physicsObject.PhysicsShape, out firstIntersect, options.ObstacleLayerMask);
                }

                if (options.UseOnGroundDetection && !flag)
                {
                    if (!physicsObject.IsOnGroundTest())
                        flag = true;
                }

                physicsObject.transform.position = cachePosition;

                if (flag)
                {
                    if (currentDeepIterateCount <= options.IntersectDeepIterateCount)
                    {
                        step *= 0.5f;
                        currentDeepIterateCount++;
                    }
                    else
                    {
                        result = false;
                        canNext = false;
                    }
                }
                else
                {
                    physicsObject.transform.position += step;//执行一次移动
                }

                if (canNext)//距离跳出判断
                {
                    canNext = Vector3.Distance(physicsObject.transform.position, to) > options.Eps;
                }

                if (canNext)//方向跳出判断
                {
                    var currentDirection = (to - physicsObject.transform.position).normalized;
                    var dot = Vector3.Dot(direction, currentDirection);
                    if (dot < 0.1f)
                    {
                        canNext = false;
                    }
                }
            }

            return result;
        }

        static bool StepMoveCastOptimize(PhysicsObject physicsObject, Vector3 moveValue, StepMoveOptions options, out Vector3 newLocation)
        {
            //运动Bounds比原始Bounds大一圈，并且长宽高一致。
            //然后做一次LookAt.
            //这个对象是一个Shape，也可以像Check那样缓存一个Shape做测试。
            //后面要追加二分法，就开始不断地修改大小。Options里需要加一个字段，最小的二分值，以及用不用二分。
            //二分分到最小二分值的时候就可以停止了。
            newLocation = Vector3.zero;

            var bounds = physicsObject.PhysicsShape.Bounds;
            StepMoveCastBoxTestShape.matrix = Matrix4x4.Rotate(Quaternion.LookRotation(moveValue.normalized));
            StepMoveCastBoxTestShape.size = bounds.size;

            return false;
        }

        public static bool BoxVSBox(Vector3 xP0, Vector3 xP1, Vector3 xP2, Vector3 xP3, Vector3 xP4, Vector3 xP5, Vector3 xP6, Vector3 xP7
                                                    , Vector3 yP0, Vector3 yP1, Vector3 yP2, Vector3 yP3, Vector3 yP4, Vector3 yP5, Vector3 yP6, Vector3 yP7
                                                    , Vector3 x_XAxis, Vector3 x_YAxis, Vector3 x_ZAxis
                                                    , Vector3 y_XAxis, Vector3 y_YAxis, Vector3 y_ZAxis)
        {
            var isNotIntersect = false;
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, x_XAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, x_YAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, x_ZAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, y_XAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, y_YAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, y_ZAxis);

            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_XAxis, y_XAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_XAxis, y_YAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_XAxis, y_ZAxis));

            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_YAxis, y_XAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_YAxis, y_YAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_YAxis, y_ZAxis));

            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_ZAxis, y_XAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_ZAxis, y_YAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_ZAxis, y_ZAxis));

            return isNotIntersect ? false : true;
        }

        public static bool BoxVSSphere(Matrix4x4 xMatrix, Vector3 xOffset, Vector3 xSize, Vector3 yCenter, float yRadius)
        {
            var localSpherePoint = xMatrix.inverse.MultiplyPoint3x4(yCenter);

            var sizeHalf = xSize * 0.5f;

            var xMin = xOffset.x + (-sizeHalf.x);
            var xMax = xOffset.x + sizeHalf.x;
            var yMin = xOffset.y + (-sizeHalf.y);
            var yMax = xOffset.y + sizeHalf.y;
            var zMin = xOffset.z + (-sizeHalf.z);
            var zMax = xOffset.z + sizeHalf.z;

            var outside = false;
            if (localSpherePoint.x < xMin)
            {
                outside = true;
                localSpherePoint.x = xMin;
            }
            else if (localSpherePoint.x > xMax)
            {
                outside = true;
                localSpherePoint.x = xMax;
            }

            if (localSpherePoint.y < yMin)
            {
                outside = true;
                localSpherePoint.y = yMin;
            }
            else if (localSpherePoint.y > yMax)
            {
                outside = true;
                localSpherePoint.y = yMax;
            }

            if (localSpherePoint.z < zMin)
            {
                outside = true;
                localSpherePoint.z = zMin;
            }
            else if (localSpherePoint.z > zMax)
            {
                outside = true;
                localSpherePoint.z = zMax;
            }

            if (outside)
            {
                var edgePoint = xMatrix.MultiplyPoint3x4(localSpherePoint);
                var distance = Vector3.Distance(yCenter, edgePoint);

                if (distance > yRadius)
                    return false;
            }

            return true;
        }

        public static bool SphereVSSphere(Vector3 xCenter, float xRadius, Vector3 yCenter, float yRadius)
        {
            return Vector3.Distance(xCenter, yCenter) <= (xRadius + yRadius);
        }

        public static bool BoxVSVirtualCapsule(Matrix4x4 xMatrix, Vector3 xOffset, Vector3 xSize
                                                                    , Matrix4x4 yMatrix, Vector3 yOffset, float yHeight, float yRadius
                                                                    , VirtualCapsuleShape.EDirection yDirection, int yVirtualSegment)
        {
            var up = Vector3.up;

            switch (yDirection)
            {
                case VirtualCapsuleShape.EDirection.X:
                    up = Vector3.right;
                    break;
                case VirtualCapsuleShape.EDirection.Z:
                    up = Vector3.forward;
                    break;
            }

            var lossyScale = yMatrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            yRadius *= maxScale;
            yHeight *= maxScale;

            var remainHeight = Mathf.Max(yHeight - yRadius * 2f, 0);
            var remainHeightHalf = remainHeight * 0.5f;
            up = yMatrix.MultiplyVector(up);
            yOffset = yMatrix.MultiplyPoint3x4(yOffset);

            if (BoxVSSphere(xMatrix, xOffset, xSize, yOffset + up * remainHeightHalf, yRadius))
                return true;

            if (BoxVSSphere(xMatrix, xOffset, xSize, yOffset - up * remainHeightHalf, yRadius))
                return true;

            if (yVirtualSegment < 3)
                return false;

            var bottom = yOffset - remainHeightHalf * up;
            var top = yOffset + remainHeightHalf * up;
            var step = (top - bottom) / Mathf.Max(1, yVirtualSegment - 1);

            for (int i = 1; i <= yVirtualSegment - 1; i++)
            {
                var spherePosition = bottom + step * i;

                if (BoxVSSphere(xMatrix, xOffset, xSize, spherePosition, yRadius))
                    return true;
            }

            return false;
        }

        public static bool SphereVSVirtualCapsule(Vector3 xCenter, float xRadius
                                                                    , Matrix4x4 yMatrix, Vector3 yOffset, float yHeight, float yRadius
                                                                    , VirtualCapsuleShape.EDirection yDirection, int yVirtualSegment)
        {
            var up = Vector3.up;

            switch (yDirection)
            {
                case VirtualCapsuleShape.EDirection.X:
                    up = Vector3.right;
                    break;
                case VirtualCapsuleShape.EDirection.Z:
                    up = Vector3.forward;
                    break;
            }

            var lossyScale = yMatrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            yRadius *= maxScale;
            yHeight *= maxScale;

            var remainHeight = Mathf.Max(yHeight - yRadius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;
            up = yMatrix.MultiplyVector(up);
            yOffset = yMatrix.MultiplyPoint3x4(yOffset);

            if (SphereVSSphere(xCenter, xRadius, yOffset + up * remainHeightHalf, yRadius))
                return true;

            if (SphereVSSphere(xCenter, xRadius, yOffset - up * remainHeightHalf, yRadius))
                return true;

            if (yVirtualSegment < 3)
                return false;

            var bottom = yOffset - remainHeightHalf * up;
            var top = yOffset + remainHeightHalf * up;
            var step = (top - bottom) / Mathf.Max(1, yVirtualSegment - 1);

            for (int i = 1; i < yVirtualSegment - 1; i++)
            {
                var spherePosition = bottom + step * i;

                if (SphereVSSphere(xCenter, xRadius, spherePosition, yRadius))
                    return true;
            }

            return false;
        }

        public static bool VirtualCapsuleVSVirtualCapsule(
                                                                    Matrix4x4 xMatrix, Vector3 xOffset, float xHeight, float xRadius
                                                                    , VirtualCapsuleShape.EDirection xDirection, int xVirtualSegment

                                                                    , Matrix4x4 yMatrix, Vector3 yOffset, float yHeight, float yRadius
                                                                    , VirtualCapsuleShape.EDirection yDirection, int yVirtualSegment)
        {
            var up = Vector3.up;

            switch (xDirection)
            {
                case VirtualCapsuleShape.EDirection.X:
                    up = Vector3.right;
                    break;
                case VirtualCapsuleShape.EDirection.Z:
                    up = Vector3.forward;
                    break;
            }

            up = xMatrix.MultiplyVector(up);

            var lossyScale = xMatrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            xRadius *= maxScale;
            xHeight *= maxScale;

            var remainHeight = Mathf.Max(xHeight - xRadius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;
            xOffset = xMatrix.MultiplyPoint3x4(xOffset);

            if (SphereVSVirtualCapsule(xOffset + up * remainHeightHalf, xRadius, yMatrix, yOffset, yHeight, yRadius, yDirection, yVirtualSegment))
                return true;

            if (SphereVSVirtualCapsule(xOffset - up * remainHeightHalf, xRadius, yMatrix, yOffset, yHeight, yRadius, yDirection, yVirtualSegment))
                return true;

            if (xVirtualSegment < 3)
                return false;

            var bottom = xOffset - remainHeightHalf * up;
            var top = xOffset + remainHeightHalf * up;
            var step = (top - bottom) / Mathf.Max(1, xVirtualSegment - 1);

            for (int i = 1; i < xVirtualSegment - 1; i++)
            {
                var spherePosition = bottom + step * i;

                if (SphereVSVirtualCapsule(spherePosition, xRadius, yMatrix, yOffset, yHeight, yRadius, yDirection, yVirtualSegment))
                    return true;
            }

            return false;
        }

        public static bool Linecast(Vector3 lineBegin, Vector3 lineEnd, out Vector3 intersectPoint, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            var min = float.MaxValue;
            var returnPoint = default(Vector3?);
            intersectPoint = Vector3.zero;

            if (containColliderObjects)
            {
                var colliderObjects = default(IList<ColliderObject>);

                if (Application.isPlaying)
                    colliderObjects = PhysicsSystemManager.Instance.ColliderObjectList;
                else
                    colliderObjects = UnityEngine.Object.FindObjectsOfType<ColliderObject>();

                for (int i = 0, iMax = colliderObjects.Count; i < iMax; i++)
                {
                    var colliderObject = colliderObjects[i];

                    if (LinecastIntersect(colliderObject, lineBegin, lineEnd, layerMask, out Vector3 point))
                    {
                        var currentDistance = Vector3.Distance(lineBegin, point);

                        if (currentDistance < min)
                        {
                            returnPoint = point;
                            min = currentDistance;
                        }
                    }
                }
            }//if (containColliderObjects)

            if (containPhysicsObjects)
            {
                if (Application.isPlaying)
                {
                    PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(physicsObject =>
                    {
                        if (LinecastIntersect(physicsObject, lineBegin, lineEnd, layerMask, out PhysicsHit hit))
                        {
                            var currentDistance = Vector3.Distance(lineBegin, hit.Point);

                            if (currentDistance < min)
                            {
                                returnPoint = hit.Point;
                                min = currentDistance;
                            }
                        }

                        return true;

                    }, layerMask);
                }
                else
                {
                    var physicsObjects = UnityEngine.Object.FindObjectsOfType<PhysicsObject>();

                    for (int i = 0, iMax = physicsObjects.Length; i < iMax; i++)
                    {
                        var item = physicsObjects[i];

                        if (LinecastIntersect(item, lineBegin, lineEnd, layerMask, out Vector3 point))
                        {
                            var currentDistance = Vector3.Distance(lineBegin, point);

                            if (currentDistance < min)
                            {
                                returnPoint = point;
                                min = currentDistance;
                            }
                        }
                    }
                }
            }//if (containPhysicsObjects)

            if (returnPoint != null)
            {
                intersectPoint = returnPoint.Value;
                return true;
            }
            else
            {
                return false;
            }
        }

        public static bool Linecast(Vector3 lineBegin, Vector3 lineEnd, out PhysicsHit hit, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            var min = float.MaxValue;
            var returnHit = default(PhysicsHit?);
            hit = default(PhysicsHit);

            if (containColliderObjects)
            {
                var colliderObjects = default(IList<ColliderObject>);

                if (Application.isPlaying)
                    colliderObjects = PhysicsSystemManager.Instance.ColliderObjectList;
                else
                    colliderObjects = UnityEngine.Object.FindObjectsOfType<ColliderObject>();

                for (int i = 0, iMax = colliderObjects.Count; i < iMax; i++)
                {
                    var colliderObject = colliderObjects[i];

                    if (LinecastIntersect(colliderObject, lineBegin, lineEnd, layerMask, out PhysicsHit _hit))
                    {
                        var currentDistance = Vector3.Distance(lineBegin, _hit.Point);

                        if (currentDistance < min)
                        {
                            returnHit = _hit;
                            min = currentDistance;
                        }
                    }
                }
            }//if (containColliderObjects)

            if (containPhysicsObjects)
            {
                if (Application.isPlaying)
                {
                    PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(physicsObject =>
                    {
                        if (LinecastIntersect(physicsObject, lineBegin, lineEnd, layerMask, out PhysicsHit _hit))
                        {
                            var currentDistance = Vector3.Distance(lineBegin, _hit.Point);

                            if (currentDistance < min)
                            {
                                returnHit = _hit;
                                min = currentDistance;
                            }
                        }

                        return true;

                    }, layerMask);
                }
                else
                {
                    var physicsObjects = UnityEngine.Object.FindObjectsOfType<PhysicsObject>();

                    for (int i = 0, iMax = physicsObjects.Length; i < iMax; i++)
                    {
                        var item = physicsObjects[i];

                        if (LinecastIntersect(item, lineBegin, lineEnd, layerMask, out PhysicsHit _hit))
                        {
                            var currentDistance = Vector3.Distance(lineBegin, _hit.Point);

                            if (currentDistance < min)
                            {
                                returnHit = _hit;
                                min = currentDistance;
                            }
                        }
                    }
                }
            }//if (containPhysicsObjects)

            if (returnHit != null)
            {
                hit = returnHit.Value;
                return true;
            }
            else
            {
                return false;
            }
        }

        public static bool Raycast(Ray ray, out Vector3 intersectPoint, float distance = -1, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            return Linecast(ray.origin, ray.origin + (distance < 0 ? ray.direction * VIRTUAL_RAY : ray.direction * distance), out intersectPoint, containColliderObjects, containPhysicsObjects, layerMask);
        }

        public static bool Raycast(Ray ray, out PhysicsHit physicsHit, float distance = -1, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            return Linecast(ray.origin, ray.origin + (distance < 0 ? ray.direction * VIRTUAL_RAY : ray.direction * distance), out physicsHit, containColliderObjects, containPhysicsObjects, layerMask);
        }

        public static bool Raycast_Bvh(Ray ray, out Vector3 intersectPoint, int bvhID)
        {
            return Raycast_Bvh(ray, VIRTUAL_RAY, out intersectPoint, bvhID);
        }

        public static bool Raycast_Bvh(Ray ray, out PhysicsHit hit, int bvhID)
        {
            return Raycast_Bvh(ray, VIRTUAL_RAY, out hit, bvhID);
        }

        public static bool Raycast_Bvh(Ray ray, float distance, out Vector3 intersectPoint, int bvhID)
        {
            var isIntersect = false;
            intersectPoint = Vector3.zero;

            var container = BVHTreeContainer.GetContainerWithID(bvhID);
            var count = container.Tree.IntersectRay(ray, mBVHQueryCacheArray, distance);

            for (int i = 0; i < count; i++)
            {
                isIntersect = mBVHQueryCacheArray[i].PhysicsShape.IntersectsTest(ray.origin, ray.origin + ray.direction * distance, out intersectPoint);
                if (isIntersect)
                    break;
            }

            return isIntersect;
        }

        public static bool Raycast_Bvh(Ray ray, float distance, out PhysicsHit hit, int bvhID)
        {
            var isIntersect = false;
            hit = default(PhysicsHit);

            var container = BVHTreeContainer.GetContainerWithID(bvhID);
            var count = container.Tree.IntersectRay(ray, mBVHQueryCacheArray, distance);

            var d = float.MaxValue;
            for (int i = 0; i < count; i++)
            {
                var r = mBVHQueryCacheArray[i].PhysicsShape.IntersectsTest(ray.origin, ray.origin + ray.direction * distance, out PhysicsHit temp_hit);
                if (r)
                {
                    var compare_d = Vector3.Distance(temp_hit.Point, ray.origin);
                    if (compare_d < d)
                    {
                        d = compare_d;
                        hit = temp_hit;
                    }

                    isIntersect = true;
                }
            }

            return isIntersect;
        }

        public static bool Linecast_Bvh(Vector3 lineBegin, Vector3 lineEnd, out Vector3 intersectPoint, int bvhID)
        {
            var d = lineEnd - lineBegin;
            return Raycast_Bvh(new Ray(lineBegin, d.normalized), d.magnitude, out intersectPoint, bvhID);
        }

        public static bool Linecast_Bvh(Vector3 lineBegin, Vector3 lineEnd, out PhysicsHit hit, int bvhID)
        {
            var d = lineEnd - lineBegin;
            return Raycast_Bvh(new Ray(lineBegin, d.normalized), d.magnitude, out hit, bvhID);
        }

        public static RaycastHit[] RaycastAll(Ray ray, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            var result = new List<RaycastHit>();

            RaycastAll(result, ray, containColliderObjects, containPhysicsObjects, layerMask);

            return result.ToArray();
        }

        public static bool CheckBox(Vector3 position, Vector3 size, Quaternion rotation, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            CheckBoxTestShape.offset = Vector3.zero;
            CheckBoxTestShape.size = size;
            CheckBoxTestShape.matrix = Matrix4x4.TRS(position, rotation, Vector3.one);
            var checkBox_bounds = CheckBoxTestShape.Bounds;

            if (containColliderObjects)
            {
                var colliderObjects = default(IList<ColliderObject>);

                if (Application.isPlaying)
                    colliderObjects = PhysicsSystemManager.Instance.ColliderObjectList;
                else
                    colliderObjects = UnityEngine.Object.FindObjectsOfType<ColliderObject>();

                for (int i = 0, iMax = colliderObjects.Count; i < iMax; i++)
                {
                    var item = colliderObjects[i];

                    if (CheckShape(item, checkBox_bounds, CheckBoxTestShape, layerMask))
                        return true;
                }
            }//if (containColliderObjects)

            if (containPhysicsObjects)
            {
                if (Application.isPlaying)
                {
                    var checkResult = false;
                    PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(physicsObject =>
                    {
                        if (CheckShape(physicsObject, checkBox_bounds, CheckBoxTestShape, layerMask))
                        {
                            checkResult = true;
                            return false;
                        }
                        else
                        {
                            return true;
                        }
                    }, layerMask);

                    if (checkResult) return true;
                }
                else
                {
                    var physicsObjects = UnityEngine.Object.FindObjectsOfType<PhysicsObject>();

                    for (int i = 0, iMax = physicsObjects.Length; i < iMax; i++)
                    {
                        var item = physicsObjects[i];

                        if (CheckShape(item, checkBox_bounds, CheckBoxTestShape, layerMask))
                        {
                            return true;
                        }
                    }
                }
            }//if (containPhysicsObjects)

            return false;
        }

        public static bool CheckBox_Bvh(Vector3 position, Vector3 size, Quaternion rotation, int bvhID)
        {
            CheckBoxTestShape.offset = Vector3.zero;
            CheckBoxTestShape.size = size;
            CheckBoxTestShape.matrix = Matrix4x4.TRS(position, rotation, Vector3.one);
            var checkBox_bounds = CheckBoxTestShape.Bounds;

            var container = BVHTreeContainer.GetContainerWithID(bvhID);
            var count = container.Tree.Intersect(checkBox_bounds, mBVHQueryCacheArray);

            for (int i = 0; i < count; i++)
            {
                if (CheckShape(mBVHQueryCacheArray[i], checkBox_bounds, CheckBoxTestShape, null))
                    return true;
            }

            return false;
        }

        public static bool CheckSphere(Vector3 position, float radius, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            CheckSphereTestShape.offset = position;
            CheckSphereTestShape.radius = radius;
            var checkSphere_bounds = new Bounds(position, Vector3.one * radius);

            if (containColliderObjects)
            {
                var colliderObjects = default(IList<ColliderObject>);

                if (Application.isPlaying)
                    colliderObjects = PhysicsSystemManager.Instance.ColliderObjectList;
                else
                    colliderObjects = UnityEngine.Object.FindObjectsOfType<ColliderObject>();

                for (int i = 0, iMax = colliderObjects.Count; i < iMax; i++)
                {
                    var item = colliderObjects[i];

                    if (CheckShape(item, checkSphere_bounds, CheckSphereTestShape, layerMask))
                        return true;
                }
            }//if (containColliderObjects)

            if (containPhysicsObjects)
            {
                if (Application.isPlaying)
                {
                    var checkResult = false;
                    PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(physicsObject =>
                    {
                        if (CheckShape(physicsObject, checkSphere_bounds, CheckSphereTestShape, layerMask))
                        {
                            checkResult = true;
                            return false;
                        }
                        else
                        {
                            return true;
                        }
                    }, layerMask);

                    if (checkResult) return true;
                }
                else
                {
                    var physicsObjects = UnityEngine.Object.FindObjectsOfType<PhysicsObject>();

                    for (int i = 0, iMax = physicsObjects.Length; i < iMax; i++)
                    {
                        var item = physicsObjects[i];

                        if (CheckShape(item, checkSphere_bounds, CheckSphereTestShape, layerMask))
                        {
                            return true;
                        }
                    }
                }
            }//if (containPhysicsObjects)

            return false;
        }

        public static bool CheckSphere_Bvh(Vector3 position, float radius, int bvhID)
        {
            var checkSphere_bounds = new Bounds(position, Vector3.one * radius);

            var container = BVHTreeContainer.GetContainerWithID(bvhID);
            var count = container.Tree.Intersect(checkSphere_bounds, mBVHQueryCacheArray);

            for (int i = 0; i < count; i++)
            {
                if (CheckShape(mBVHQueryCacheArray[i], checkSphere_bounds, CheckSphereTestShape, null))
                    return true;
            }

            return false;
        }

        public static bool CheckCapsule(Vector3 position, float radius, float height, int virtualSegment, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
        {
            CheckCapsuleTestShape.offset = position;
            CheckCapsuleTestShape.radius = radius;
            CheckCapsuleTestShape.height = height;
            CheckCapsuleTestShape.virtualSegment = virtualSegment;

            var checkCapsule_bounds = CheckCapsuleTestShape.Bounds;

            if (containColliderObjects)
            {
                var colliderObjects = default(IList<ColliderObject>);

                if (Application.isPlaying)
                    colliderObjects = PhysicsSystemManager.Instance.ColliderObjectList;
                else
                    colliderObjects = UnityEngine.Object.FindObjectsOfType<ColliderObject>();

                for (int i = 0, iMax = colliderObjects.Count; i < iMax; i++)
                {
                    var item = colliderObjects[i];

                    if (CheckShape(item, checkCapsule_bounds, CheckCapsuleTestShape, layerMask))
                        return true;
                }
            }//if (containColliderObjects)

            if (containPhysicsObjects)
            {
                if (Application.isPlaying)
                {
                    var checkResult = false;
                    PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(physicsObject =>
                    {
                        if (CheckShape(physicsObject, checkCapsule_bounds, CheckCapsuleTestShape, layerMask))
                        {
                            checkResult = true;
                            return false;
                        }
                        else
                        {
                            return true;
                        }
                    }, layerMask);

                    if (checkResult) return true;
                }
                else
                {
                    var physicsObjects = UnityEngine.Object.FindObjectsOfType<PhysicsObject>();

                    for (int i = 0, iMax = physicsObjects.Length; i < iMax; i++)
                    {
                        var item = physicsObjects[i];

                        if (CheckShape(item, checkCapsule_bounds, CheckCapsuleTestShape, layerMask))
                        {
                            return true;
                        }
                    }
                }
            }//if (containPhysicsObjects)

            return false;
        }

        public static bool CheckCapsule_Bvh(Vector3 position, float radius, float height, int virtualSegment, int bvhID)
        {
            CheckCapsuleTestShape.offset = position;
            CheckCapsuleTestShape.radius = radius;
            CheckCapsuleTestShape.height = height;
            CheckCapsuleTestShape.virtualSegment = virtualSegment;

            var checkCapsule_bounds = CheckCapsuleTestShape.Bounds;

            var container = BVHTreeContainer.GetContainerWithID(bvhID);
            var count = container.Tree.Intersect(checkCapsule_bounds, mBVHQueryCacheArray);

            for (int i = 0; i < count; i++)
            {
                if (CheckShape(mBVHQueryCacheArray[i], checkCapsule_bounds, CheckCapsuleTestShape, null))
                    return true;
            }

            return false;
        }

        public static Vector3? GetIntersectPoint(Vector3 planeNormal, Vector3 planePosition, Vector3 p0, Vector3 p1)
        {
            var sign1 = Mathf.Sign(Vector3.Dot(planeNormal, planePosition - p0));
            var sign2 = Mathf.Sign(Vector3.Dot(planeNormal, planePosition - p1));
            if (Mathf.Approximately(sign1, sign2)) return null;//同侧异侧.

            var a = planeNormal.x;
            var b = planeNormal.y;
            var c = planeNormal.z;
            var d = -a * planePosition.x - b * planePosition.y - c * planePosition.z;

            var i0 = a * p0.x + b * p0.y + c * p0.z;
            var i1 = a * p1.x + b * p1.y + c * p1.z;
            var final_t = -(i1 + d) / (i0 - i1);

            var finalPoint = new Vector3()
            {
                x = p0.x * final_t + p1.x * (1 - final_t),
                y = p0.y * final_t + p1.y * (1 - final_t),
                z = p0.z * final_t + p1.z * (1 - final_t),
            };

            return finalPoint;
        }

        static void RaycastAll<TList>(TList list, Ray ray, bool containColliderObjects = true, bool containPhysicsObjects = false, int layerMask = ~0)
            where TList : IList<RaycastHit>
        {
            const float VIRTUAL_RAY = 10__0000;

            if (containColliderObjects)
            {
                var colliderObjects = default(IList<ColliderObject>);

                if (Application.isPlaying)
                    colliderObjects = PhysicsSystemManager.Instance.ColliderObjectList;
                else
                    colliderObjects = UnityEngine.Object.FindObjectsOfType<ColliderObject>();

                for (int i = 0, iMax = colliderObjects.Count; i < iMax; i++)
                {
                    var item = colliderObjects[i];

                    if (LinecastIntersect(item, ray.origin, ray.origin + ray.direction * VIRTUAL_RAY, layerMask, out Vector3 point))
                        list.Add(new RaycastHit() { ContactPoint = point, Object = item });
                }
            }// if (containColliderObjects)

            if (containPhysicsObjects)
            {
                if (Application.isPlaying)
                {
                    PhysicsSystemManager.Instance.ForeachRegistedPhysicsObjects(physicsObject =>
                    {
                        if (LinecastIntersect(physicsObject, ray.origin, ray.origin + ray.direction * VIRTUAL_RAY, layerMask, out Vector3 point))
                            list.Add(new RaycastHit() { ContactPoint = point, Object = physicsObject });

                        return true;

                    }, layerMask);
                }
                else
                {
                    var physicsObjects = UnityEngine.Object.FindObjectsOfType<PhysicsObject>();
                    for (int i = 0, iMax = physicsObjects.Length; i < iMax; i++)
                    {
                        var physicsObject = physicsObjects[i];

                        if (LinecastIntersect(physicsObject, ray.origin, ray.origin + ray.direction * VIRTUAL_RAY, layerMask, out Vector3 point))
                            list.Add(new RaycastHit() { ContactPoint = point, Object = physicsObject });
                    }
                }
            }// if (containPhysicsObjects)
        }

        static bool CheckShape(PhysicsBase physicsBase, Bounds cacheBounds, IPhysicsShape cachePhysicsShape, int? layerMask)
        {
            if (layerMask != null && (physicsBase.Layer & layerMask) != physicsBase.Layer)
                return false;

            var colliderObjectPhysicsShape = physicsBase.PhysicsShape;

            if (!colliderObjectPhysicsShape.Bounds.Intersects(cacheBounds))
                return false;

            if (!colliderObjectPhysicsShape.IntersectsTest(cachePhysicsShape))
                return false;

            return true;
        }

        static bool LinecastIntersect(PhysicsBase physicsBase, Vector3 lineBegin, Vector3 lineEnd, int layerMask, out Vector3 intersectPoint)
        {
            intersectPoint = Vector3.zero;

            var selfMask = 1 << physicsBase.Layer;
            if ((selfMask & layerMask) != selfMask) return false;

            return physicsBase.PhysicsShape.IntersectsTest(lineBegin, lineEnd, out intersectPoint);
        }

        static bool LinecastIntersect(PhysicsBase physicsBase, Vector3 lineBegin, Vector3 lineEnd, int layerMask, out PhysicsHit physicsHit)
        {
            physicsHit = default(PhysicsHit);

            var selfMask = 1 << physicsBase.Layer;
            if ((selfMask & layerMask) != selfMask) return false;

            return physicsBase.PhysicsShape.IntersectsTest(lineBegin, lineEnd, out physicsHit);
        }

        static bool ProjectionIsNotIntersect(Vector3 xP0, Vector3 xP1, Vector3 xP2, Vector3 xP3, Vector3 xP4, Vector3 xP5, Vector3 xP6, Vector3 xP7
                                                                  , Vector3 yP0, Vector3 yP1, Vector3 yP2, Vector3 yP3, Vector3 yP4, Vector3 yP5, Vector3 yP6, Vector3 yP7
                                                                  , Vector3 axis)
        {
            var x_p0 = GetProject_Fast(xP0, axis);
            var x_p1 = GetProject_Fast(xP1, axis);
            var x_p2 = GetProject_Fast(xP2, axis);
            var x_p3 = GetProject_Fast(xP3, axis);
            var x_p4 = GetProject_Fast(xP4, axis);
            var x_p5 = GetProject_Fast(xP5, axis);
            var x_p6 = GetProject_Fast(xP6, axis);
            var x_p7 = GetProject_Fast(xP7, axis);

            var y_p0 = GetProject_Fast(yP0, axis);
            var y_p1 = GetProject_Fast(yP1, axis);
            var y_p2 = GetProject_Fast(yP2, axis);
            var y_p3 = GetProject_Fast(yP3, axis);
            var y_p4 = GetProject_Fast(yP4, axis);
            var y_p5 = GetProject_Fast(yP5, axis);
            var y_p6 = GetProject_Fast(yP6, axis);
            var y_p7 = GetProject_Fast(yP7, axis);

            var xMin = Mathf.Min(x_p0, Mathf.Min(x_p1, Mathf.Min(x_p2, Mathf.Min(x_p3, Mathf.Min(x_p4, Mathf.Min(x_p5, Mathf.Min(x_p6, x_p7)))))));
            var xMax = Mathf.Max(x_p0, Mathf.Max(x_p1, Mathf.Max(x_p2, Mathf.Max(x_p3, Mathf.Max(x_p4, Mathf.Max(x_p5, Mathf.Max(x_p6, x_p7)))))));
            var yMin = Mathf.Min(y_p0, Mathf.Min(y_p1, Mathf.Min(y_p2, Mathf.Min(y_p3, Mathf.Min(y_p4, Mathf.Min(y_p5, Mathf.Min(y_p6, y_p7)))))));
            var yMax = Mathf.Max(y_p0, Mathf.Max(y_p1, Mathf.Max(y_p2, Mathf.Max(y_p3, Mathf.Max(y_p4, Mathf.Max(y_p5, Mathf.Max(y_p6, y_p7)))))));

            if (yMin >= xMin && yMin <= xMax) return false;
            if (yMax >= xMin && yMax <= xMax) return false;
            if (xMin >= yMin && xMin <= yMax) return false;
            if (xMax >= yMin && xMax <= yMax) return false;

            return true;
        }

        static float GetProject_Fast(Vector3 point, Vector3 onNormal)
        {
            return Vector3.Dot(point, onNormal);

            //var projectPoint = Vector3.Project(point, onNormal);
            //var result = projectPoint.magnitude * Mathf.Sign(Vector3.Dot(projectPoint, onNormal));

            //return result;
        }

    }
}
